addpath build
mathematica_path = ...
    ['C:' filesep ...
    'Program Files' filesep ...
    'Wolfram Research' filesep ...
    'Mathematica' filesep ...
    '8.0' filesep];
main_path = [pwd filesep];%...
%    ['C:' filesep ...
%    'Users' filesep ...
%    'rsinnet' filesep ...
%    'Documents' filesep ...
%    'MATLAB' filesep ...
%    'meen-652-biped' filesep ...
%    'code' filesep];
st = load('traj.mat', 'traj');
traj = st.traj;

N = length(traj.t);
ndof = fix(size(traj.x, 1) / 2);

t = traj.t;
x0 = traj.x;
phip = traj.phip;

A = nan(2*ndof, 2*ndof, N-1);
B = nan(2*ndof, ndof, N-1);
C = nan(ndof, 2*ndof, N-1);
D = nan(ndof, ndof, N-1);

K = nan(ndof, 2*ndof, N-1);
Q = nan(2*ndof, 2*ndof, N-1);
R = nan(ndof, ndof, N-1);

for k = 1:N
    xk = x0(:, k);
    save('xk.mat', 'xk');
    %system('del .\\build\\lsys\ss.mat');
    system(...
        ['"' mathematica_path 'math.exe" ' ...
        '-noprompt -run "<<' ...
        main_path 'doLinearization.txt; Quit[];"']);
    st = load(...
        ['.' filesep 'build' filesep 'lsys' filesep 'ss.mat'], ...
        'A', 'B', 'C', 'D');
    
    A(:, :, k) = st.A;
    B(:, :, k) = st.B;
    C(:, :, k) = st.C;
    D(:, :, k) = st.D;
    
    Q(:, :, k) = C(:, :, k)' * C(:, :, k);
    R(:, :, k) = 1/100*eye(ndof);
    
    sys = ss(st.A, st.B, st.C, st.D);
    K(:, :, k) = lqr(sys, Q(:, :, k), R(:, :, k));
    
    fprintf('Completion: %2.2f%%\n', 100*k/N);
end

save('traj_ss_lsys.mat',...
    't', 'phip', 'x0', ...
    'A', 'B', 'C', 'D', ...
    'K');
